/***********************************************************
 * Matrix Mathematics
 * Programmers:
 *		Cory Frey
 *		Kendra Konopka
 *		Charles Leichner
 *		Andrew Weatherly
 *
 * Date:
 *		6 December 2010
 *
 * Description:
 *		This file implements the complex mathematical functions
 *		of the Matrix.h library. 
 *
 ***********************************************************/

#include "Matrix.h"
#include "MatlabPrinter.h"
#include <iostream>
#include <fstream>
#include <string>
#include <exception>
#include <cmath>
#include <sstream>
#include <cassert>

	/****************************************************************/
	//                Functions of OurMatrix Class                  //
	/****************************************************************/
// isSquare checks to see if the matrix it is called on is square and returns a bool
	// Use: square = your_matrix.isSquare()
bool Matrix::isSquare(){
    return this->numRows == this-> numCols;
}

 // Inverse function will return the inverse of the matrix it is called on
		// Use:  inv_your_matrix = your_matrix.inverse()
Matrix Matrix::inverse(){
	MatlabPrinter::inverse(*this);
	if(this->determinant() != 0){
		Matrix result;   
		if (numRows == 1){// WORKS!!!
			// this is a 1 x 1 matrix
			result = Matrix(numRows, numCols);
			result(0,0) = 1 / (*this)(0,0);
		}  
		else if (numRows == 2) {// WORKS!!!
			// this is a 2 x 2 matrix
			result = Matrix(numRows, numCols);
			result(0,0) = (*this)(1,1);
			result(0,1) = -(*this)(0,1);
			result(1,0) = -(*this)(1,0);
			result(1,1) = (*this)(0,0);
			result = (1/(this->determinant())) * result;
		}
		else {// WORKS!!!
			// Use Gauss-Jordan method to find inverse	
			result = this->identity(numRows);
			Matrix augmented = this->augment(result);
			augmented = augmented.rref();
			for(int i = 0; i < result.numRows; i++){
				for(int j = 0; j < result.numCols; j++){
					result(i,j) = augmented(i,j+numCols);
				}
			}
		}
		return result;
	}
	else{
        std::cerr << "Singular Matrix" << std::endl;
		throw "Singular Matrix";
	}
}

// rref function will return the Reduced row echelon form of the matrix it is called on
		// Use:  rref_your_matrix = your_matrix.rref()
Matrix Matrix::rref(){
	MatlabPrinter::echelon(*this);
	Matrix result(numRows, numCols);
	result = *this;
	for (int col = 0; col < numCols; col++){
		// We want the elements on the diagonal to be nonzero
		// Find the first row with a non-zero value in column col
		int row;
		if (col < numRows){
			for (row = col; row < numRows &&  result(row, col) == 0; row++);
			if (row != col && row < numRows){
				// This implies that the element on the diagonal is 0
				// Swap other elements
				for (int swapIndex = 0; swapIndex < numCols; swapIndex++){
					double temp;
					temp = result(col, swapIndex);
					result(col, swapIndex) = result(row, swapIndex);
					result(row, swapIndex) = temp;
				}
			}
			// Remove non-zero values on all rows at column col
			for (int row = 0; row < numRows; row++){
				if (row != col){
	
					// Remove this value by adding a multiple of diagonal
					if (result(row,col) != 0 && result(col,col) != 0){				
						double removeFactor = -result(row, col) / result(col,col);
						result.addRowMultiple(row,col,removeFactor);
					}			
				}
				else{
					// We want the value of the diagonals to be 1
					double dividingFactor = result(col,col);
					if (result(col,col) != 0){
						for (int dividingIndex = 0; dividingIndex < numCols; dividingIndex++){
							result(row, dividingIndex) = result(row, dividingIndex) / dividingFactor;
						}
					}
				}//end else
			}//end for loop
		}
	}//end for loop

	return result;
}

// Eigen Value function will return a vector full of the Eigen values
EigenVals Matrix::eigenValues(){
	MatlabPrinter::eigenValues(*this);
	EigenVals values;
	if (!isSquare()){
		std::stringstream out;
		std::cout<<"Unable to Compute the Eigen values of a "<<numRows<<"x"<<numCols<<" matrix, the matrix must be square.\n";
		out<<"Unable to Compute the Eigen values of a "<<numRows<<"x"<<numCols<<" matrix, the matrix must be square.\n";
		throw out.str();
		}
	else{
		if(numRows == 1){
			values.push_back((*this)(0,0));
		}
		else if(numCols == 2){
			double a=0,b=0,c=0, arg=0, val1=0, val2=0;
			a = 1;
			b = -((*this).trace());
			c = (*this).determinant();

			arg = b*b-(4*a*c);
			// Complex roots:
			if(arg<0){
                std::stringstream out;
                std::cout<<"I am currently unable to Compute the Eigen values of the "<<numRows<<"x"<<numCols<<" matrix, becasue its roots are complex.\n";
				out<<"I am currently unable to Compute the Eigen values of the "<<numRows<<"x"<<numCols<<" matrix, becasue its roots are complex.\n";
				throw out.str();
			}
			else{
				// (-b+sqrt(arg))/2a
				val1 = (-b+sqrt(arg))/(2*a);
				values.push_back(val1);
				// (-b-sqrt(arg))/2a
				val2 = (-b-sqrt(arg))/(2*a);
				values.push_back(val2);
			}
		}
		else if(numCols == 3){
			// Using Method from http://www.1728.com/cubic2.htm
			double a=0,b=0,c=0,d=0;
			double f=0, g=0, h=0;
			double val1=0, val2=0, val3=0;

			a = -1;																// a =-1
			b = (*this).trace();												// b = trace(A)
			c = (1.0/2)*((((*this).Mpow(2)).trace())-(pow((*this).trace(),2)));	// c =(1/2)(trace(A^2)-(trace(A))^2)
			d = (*this).determinant();											// d = det(A)
			f = (((3.0*c)/a)-((pow(b,2))/(pow(a,2))))/3;						// f = ((3c/a)-(b^2/a^2))/3
			g = (((2*pow(b,3))/(pow(a,3)))-((9*b*c)/pow(a,2))+((27*d)/a))/27;	// g = (((2*b^3)/(a^3))-((9*b*c)/(a^2))+((27*d)/a))/27
			h = ((pow(g,2))/4)+((pow(f,3))/27);									// h = (g^2)/4 + (f^3)/27

			// The value of h determines the types of roots:
				// h <= 0 all three roots are real Method A
				// h>0: only one real root, solved my Method B
				// f=g=h=0 all three roots are real and equal Method C
			
			// Method A: 3 real roots
			if((h <= 0)&&(!((f==g)&&(g==h)&&(h==0)))){ // go into this if h<=0 and !(f=g=h=0)
				double i=0, j=0, k=0, l=0, m=0, n=0, p=0;
			
				i = pow((((pow(g,2))/4)-h),(1.0/2));									// i = ((g^2/4)-h)^(1/2)
				j = pow(i,(1.0/3));														// j = (i)^1/3
				k = acos (-1.0*(g/(2.0*i)));											// arc cosine(-(g/2*i))
				l = j*(-1);																// j*-1
				m = cos(k/3.0);															// m = cos(k/3)
				n = sqrt(3.0)*sin(k/3.0);												// n = sqrt(3)*sin(k/3)
				p = (b/(3.0*a))*-1;														// p = (b/3a)*-1

				val1 = (2.0*j*cos(k/3.0)) - (b/(3*a));									// val1 =  2*j*cos(k/3)-(b/3a)
				val2 = (l*(m+n))+p;														// val2 = l*(m+n)+p
				val3 = (l*(m-n))+p;														// val3 = l*(m-n)+p

				values.push_back(val1);
				values.push_back(val2);
				values.push_back(val3);
			}
			// Method B: One real root, and 2 imaginary ones
			else if(h>0){
                std::stringstream out;
                std::cout<<"I am currently unable to Compute the Eigen values of the "<<numRows<<"x"<<numCols<<" matrix, becasue two of its roots are complex.\n";
				out<<"I am currently unable to Compute the Eigen values of the "<<numRows<<"x"<<numCols<<" matrix, becasue two of its roots are complex.\n";
				throw out.str();
			}
			// Method C: Three real equal roots
			else{
				//f,g,h all used from above
				val1 = (pow((d/a),(1.0/3)))*-1;										//(d/a)^1/3 *-1
				val2 = val1;
				val3 = val2;

				values.push_back(val1);
				values.push_back(val2);
				values.push_back(val3);

			}
		}// end numCols==3
		// Else statment for matrices > 3x3
		else{
            std::stringstream out;
            std::cout<<"I am currently unable to Compute the Eigen values of a "<<numRows<<"x"<<numCols<<" matrix.\n";
			out<<"I am currently unable to Compute the Eigen values of a "<<numRows<<"x"<<numCols<<" matrix.\n";
			throw out.str();
		}
	return values;
	}
}

// eigenVectors will return a vector of Matrices that contain the reduced row echelon form of (A - (lambda)I).
	//Use: eigenspace = your_matrix.eigenVectors()
std::vector<Matrix> Matrix::eigenVectors(){
	MatlabPrinter::eigenVectors(*this);
	EigenVals eVals = this->eigenValues();
    std::vector<Matrix> result;
	Matrix current(numRows, numCols);
	Matrix tempI(numRows, numCols);
	for (unsigned int i = 0; i < eVals.size(); i++){
		tempI = identity(numCols);
		tempI = (eVals.at(i) * tempI);
		current = (*this) - tempI;
		result.push_back(current.rref());
	}
	return result;
}

// Echelon Function will return the Echelon form of a Matrix it is called on
	//Use: ech_your_matrix = your_matrix.echelon()
Matrix Matrix::echelon(){
	MatlabPrinter::echelon(*this);
	//Math410 way to find non-Reduced echelon form
	int b=0;
	int rows = this->numRows;
	int cols = this->numCols;
	Matrix result= (*this);
	for(int a=0; a<cols; a++, b++){
		for(int move=1;(a+move)<rows;move++){
			if(result(a,b)!=0){
				double rowa = result(a,b);
				double rowb = result(a+move,b);
				double Lfactor = rowb/rowa;
				for(int c=0; c<cols; c++){
					result(a+move,c)=result(a+move,c)-Lfactor*result(a,c);
				}
			}
		}
	}
	return result;
}

// Lfact function will return the L factorization of a matrix that it is called on
	//Use: Lfact_your_matrix = your_matrix.Lfact()
Matrix Matrix::Lfact(){
	MatlabPrinter::LUFactorization(*this);
	if(this->isSquare()){
	//Math410 L factorization
	int b=0;
	int Co=0;
	int rows = this->numRows;
	int cols = this->numCols;
	Matrix result= (*this);
	Matrix L(rows, cols);
	for(int a=0; a<cols; a++, b++){
		for(int move=1;(a+move)<rows;move++){
			if(result(a,b)!=0){
				double rowa = result(a,b);
				double rowb = result(a+move,b);
				double Lfactor = rowb/rowa;
				L(a+move,a) = Lfactor;
				for(int c=0; c<cols; c++){
					result(a+move,c)=result(a+move,c)-Lfactor*result(a,c);
				}
			}
		}
	}
	for(int I=0; I<rows;I++, Co++){
		L(I,Co)=1;
	}
	return L;
	}
	else{
        std::stringstream out;
		out << "ERROR: Matrix size is: "<< numRows <<"x"<<numCols<<"; In order to compute matrix to the L factorization, the matrix must be square.\n";
		throw out.str();
	}
}

// Ufact function will return the U factorization of a matrix that it is called on
	//Use: Ufact_your_matrix = your_matrix.Ufact()
Matrix Matrix::Ufact(){
	if(this->isSquare()){
	//Math410 U factorization
	int b=0;
	int Co=0;
	int rows = this->numRows;
	int cols = this->numCols;
	Matrix result= (*this);
	Matrix L(rows, cols);
	for(int a=0; a<cols; a++, b++){
		for(int move=1;(a+move)<rows;move++){
			if(result(a,b)!=0){
				double rowa = result(a,b);
				double rowb = result(a+move,b);
				double Lfactor = rowb/rowa;
				L(a+move,a) = Lfactor;
				for(int c=0; c<cols; c++){
					result(a+move,c)=result(a+move,c)-Lfactor*result(a,c);
				}
			}
		}		
	}
	for(int I=0; I<rows;I++, Co++){
		L(I,Co)=1;
	}
	return result;
	}
	else{
        std::stringstream out;
		out << "ERROR: Matrix size is: "<< numRows <<"x"<<numCols<<"; In order to compute matrix to the U factorization, the matrix must be square.\n";
		throw out.str();
	}
}

// Qfact function will return the Q factorization of a matrix that it is called on
	//Use: Qfact_your_matrix = your_matrix.Qfact()
Matrix Matrix::Qfact(){
	MatlabPrinter::QRFactorization(*this);
	if(this->isSquare()){
	//Math410 Q factorization
	double c=0;
	double temp=0;
	int rows = this->numRows;
	int cols = this->numCols;
	Matrix temp2(1,cols*cols);
	Matrix result= (*this);
	Matrix R(rows, cols);
	Matrix Q(rows, cols);
	for(int b=0; b<cols;b++){
		if(b==0){
			for(int a=0; a<rows; a++){	
				temp= (*this)(a,b);
				c =(temp*temp)+c;
			}
			c= sqrt(c);
			R(0,0)=c;
			for(int a=0; a<rows; a++){
				Q(a,b) =(1/c)*result(a,b);
			}
			c=0;
		}
		else{
			for(int move=1; move<=b; move++){
				for(int a=0; a<rows;a++){
					temp2(0,move+move*b)=Q(a,move-1)*result(a,b) + temp2(0,move+move*b);
					if(b!=move){
						R(move-1,b)=temp2(0,move+move*b);
					}
				}
			}
			for(int move=1; move<=b;move++){ 
				R(move-1,b)=temp2(0,move+move*b);
			}
			for(int a=0; a<rows;a++){
				Q(a,b)= temp2(0,b+b*b)*Q(a,b-1);
			}
			for(int a=0; a<rows;a++){
				Q(a,b)= result(a,b)-Q(a,b);
				for(int move=1;move<b;move++){
					Q(a,b)=Q(a,b)-Q(a,move-1)*R(move-1,b);
				}
			}
			for(int a=0; a<rows;a++){
				c=(Q(a,b)*Q(a,b))+c;
			}
			c=sqrt(c);
			R(b,b)=c;
			for(int a=0; a<rows;a++){
				Q(a,b)=(1/c)*Q(a,b);
			}
			c=0;
			temp2(0,b)=0;
		}
	}	
	return Q;
	}
	else{
        std::stringstream out;
		out << "ERROR: Matrix size is: "<< numRows <<"x"<<numCols<<"; In order to compute matrix to the Q factorization, the matrix must be square.\n";
		throw out.str();
	}
}

// Rfact function will return the R factorization of a matrix that it is called on
	//Use: Rfact_your_matrix = your_matrix.Rfact()
Matrix Matrix::Rfact(){
	if(this->isSquare()){
	//Math410 R factorization
	double c=0;
	double temp=0;
	int rows = this->numRows;
	int cols = this->numCols;
	Matrix temp2(1,cols*cols);
	Matrix result= (*this);
	Matrix R(rows, cols);
	Matrix Q(rows, cols);
	for(int b=0; b<cols;b++){
		if(b==0){
			for(int a=0; a<rows; a++){	
				temp= (*this)(a,b);
				c =(temp*temp)+c;
			}
			c= sqrt(c);
			R(0,0)=c;
			for(int a=0; a<rows; a++){
				Q(a,b) =(1/c)*result(a,b);
			}
			c=0;
		}
		else{
			for(int move=1; move<=b; move++){
				for(int a=0; a<rows;a++){
					temp2(0,move+move*b)=Q(a,move-1)*result(a,b) + temp2(0,move+move*b);
					if(b!=move){
					R(move-1,b)=temp2(0,move+move*b);
					}
				}
			}
			for(int move=1; move<=b;move++){ 
				R(move-1,b)=temp2(0,move+move*b);
			}
			for(int a=0; a<rows;a++){
				Q(a,b)= temp2(0,b+b*b)*Q(a,b-1);			
			}
			for(int a=0; a<rows;a++){
				Q(a,b)= result(a,b)-Q(a,b);
				for(int move=1;move<b;move++){
					Q(a,b)=Q(a,b)-Q(a,move-1)*R(move-1,b);
				}
			}
			for(int a=0; a<rows;a++){
				c=(Q(a,b)*Q(a,b))+c;
			}
			c=sqrt(c);
			R(b,b)=c;
			for(int a=0; a<rows;a++){
				Q(a,b)=(1/c)*Q(a,b);
			}
			c=0;
			temp2(0,b)=0;
		}
	}	
	return R;
	}
	else{
        std::stringstream out;
		out << "ERROR: Matrix size is: "<< numRows <<"x"<<numCols<<"; In order to compute matrix to the R factorization, the matrix must be square.\n";
		throw out.str();
	}
}

// Determinate function will return the determinate of the matrix it is called on
		// Use:  det_your_matrix = your_matrix.determinant()
double Matrix::determinant(){  
	MatlabPrinter::determinant(*this);
	// Declarations
    double determinant = 0;
	int rows = this->numRows;  
	int cols = this->numCols;          
    if (rows == cols){  
		// Base Case 1
        if (rows == 1){  
			determinant = (*this)(0, 0);  
        }  
		// Base Case 2
        else if (rows == 2){  
			determinant = (*this)(0, 0) * (*this)(1, 1) - (*this)(1, 0) * (*this)(0, 1);  
        }  
		// Recursive case
		else{  
			for (int c = 0; c < cols; c++){
				// make matrix smaller by removing rows.
				Matrix smallerMatrix = this->chopper(0, c);  
				determinant += pow(-1.0, c) * (*this)(0, c) * smallerMatrix.determinant();
			}
		}  
	}  
    else{  
        throw "Matrix must be square";  
    }  
    return determinant;  
}  

// Chopper function will remove the row and col passed to it, and return the resulting matrix
		// Use:  chop_your_matrix = your_matrix.chopper(row, col)
Matrix Matrix::chopper(int row, int col){  
	// Changes to one based from zero based
	row++;
	col++;
	Matrix result;  
	// Check bounds
    if (row > 0 && row <= numRows && col > 0 && col <= numCols){  
		// Declare smaller matrix
		result = Matrix(numRows - 1, numRows - 1);  
		for (int r = 1; r <= (numRows - (row >= numRows)); r++)  
			for (int c = 1; c <= (numCols - (col >= numCols)); c++)  
 				result((r-(r>row)-1), (c-(c>col)-1)) = (*mat)[r-1][c-1]; 
	}  
	else
		throw "Index for chopper out of range";    
	return result;  
}  

// Transopse function will return the a nXm matrix
Matrix Matrix::transpose(){
	MatlabPrinter::transpose(*this);
	Matrix result(numCols,numRows);
	for(int i = 0; i < numCols; i++){
		for(int j = 0; j < numRows; j++){
			result(i,j) = (*this)(j,i);
		}
	}
	return result;
}

// Trace function will calculate the trace and return it
double Matrix::trace(){
	MatlabPrinter::trace(*this);
	double trace=0;
	if(numRows==numCols){
		for (int i = 0; i < numRows; i++){
			for (int j = 0; j < numCols; j++){
				if (i == j)
					trace += (*this)(i,j);
			}
		}
		return trace;
	}
	else{
        std::stringstream out;
		out << "ERROR: Matrix size is: "<< numRows <<"x"<<numCols<<"; In order to compute the trace the matrix must be square.\n";
		throw out.str();
	}
}

// Augment function will combine the rows of two matrices
	// Use:  augment_matrix = your_matrix.augment(your_matrix2) 
Matrix Matrix::augment(Matrix rhs){
	Matrix result(numRows, numCols + rhs.numCols);
	for (int i = 0; i < result.numRows; i++){
		for (int j = 0; j < result.numCols; j++){
			if (j < numCols){
				result(i,j) = (*this)(i,j);
			}
			else{
				result(i,j) = rhs(i,j-rhs.numCols);
			}
		}
	}
	return result;
}
